/*
 *  SurfaceSuperEllipsoid.cpp
 *  EPPI-0.0
 *
 *  Created by Roberto Ortega on 18/04/11
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "surfacesuperellipsoid.h"

// ***
SurfaceSuperEllipsoid::SurfaceSuperEllipsoid()
:Surface("meshname")
{}

// ***
SurfaceSuperEllipsoid::~SurfaceSuperEllipsoid()
{}

// ***
SurfaceSuperEllipsoid::SurfaceSuperEllipsoid(ifstream &in, const string& na)
:Surface(na)
{
	rx = 1.0;
	ry = 1.0;
	rx = 1.0;
	
	aa = 0.5;
	bb = 0.5;
	
	nn = 3;
	mm = 5;

	string keyword;

	while (in >> keyword && keyword.compare("ENDSURFACE") )
	{		
		if (!keyword.compare("DIMS"))
		{
			in >> rx;
			in >> ry;
			in >> rz;
			
			cout << keyword << "\t";			
			cout << rx << "\t" << ry << "\t" << rz << endl;
		}
		if (!keyword.compare("PRMS"))
		{			
			in >> aa;
			in >> bb;
			
			cout << keyword << "\t";
			cout << aa << "\t" << bb << endl;
		}
		if (!keyword.compare("GRID"))
		{
			in >> nn;
			in >> mm;
			
			cout << keyword << "\t";
			cout << nn << "\t" << mm << endl;
		}		
		if (!keyword.compare("CONTACTPOINTS"))
		{
			in >> pointsOption;
			
			if (!pointsOption.compare("all"))
			{
				contactAllOption = true;
				
				cout << keyword << "\t";
				cout << pointsOption << endl;
			}
			
			if (!pointsOption.compare("file"))
			{
				in >> contactFileName;
				contactFileOption = true;
				
				cout << keyword << "\t";
				cout << pointsOption << "\t" << contactFileName << endl; 
			}
		}
	}
	
	createSuperEllipsoid();
	this->createContactPoints();
}
// ***
void SurfaceSuperEllipsoid::createSuperEllipsoid(void)
{
	int i;
	int j;
	
	double x;
	double y;
	double z;
	
	double u;
    double v;
	
	int elem, nelems;
    int node, nnodes;
	
	vector<double> vCoordinates;
    vector<int> vElement;

	// Make nn and mm even numbers.
	nn *= 4;
	mm *= 2;
	
	nnodes = nn * (mm - 1) + 2;
	nelems = 2 * nn * (mm - 2) + 2 * nn;
	
	// -- Nodes -- //
	vCoordinates.resize(3);
	
	//Bottom None
	u = -pii;
	v = -pii / 2.;
	
	x = rx * sgn(cos(u)) * pow(fabs(cos(u)), aa) * pow(fabs(cos(v)), bb);
	y = ry * sgn(sin(u)) * pow(fabs(sin(u)), aa) * pow(fabs(cos(v)), bb);
	z = rz * sgn(sin(v)) * pow(fabs(sin(v)), bb);
	
	vCoordinates[0] = 0;
	vCoordinates[1] = 0;
	vCoordinates[2] = z;
	
	vLocalPoints.push_back(vCoordinates);
	
	//Middle Nodes
	for (j = 0; j < mm - 1; ++j)
		for (i = 0; i < nn; ++i)
		{
			//node = i + j * (nn + 1);
			
			u = -pii + 2. * pii * i / nn;
			v = -pii / 2. + pii * (j + 1) / mm;
			
			x = rx * sgn(cos(u)) * pow(fabs(cos(u)), aa) * pow(fabs(cos(v)), bb);
			y = ry * sgn(sin(u)) * pow(fabs(sin(u)), aa) * pow(fabs(cos(v)), bb);
			z = rz * sgn(sin(v)) * pow(fabs(sin(v)), bb);
			
			//if (sin(u) == sin(-pii) or sin(u) == sin(0))
			if (i == 0 * nn / 4 or i == 2 * nn / 4)
				y = 0.;

			if (i == 1 * nn / 4 or i == 3 * nn / 4)
				x = 0.;
			
			vCoordinates[0] = x;
			vCoordinates[1] = y;
			vCoordinates[2] = z;
			
			vLocalPoints.push_back(vCoordinates);
		}

	//Top None
	u = +pii;
	v = +pii / 2.;
	
	x = rx * sgn(cos(u)) * pow(fabs(cos(u)), aa) * pow(fabs(cos(v)), bb);
	y = ry * sgn(sin(u)) * pow(fabs(sin(u)), aa) * pow(fabs(cos(v)), bb);
	z = rz * sgn(sin(v)) * pow(fabs(sin(v)), bb);
	
	vCoordinates[0] = 0;
	vCoordinates[1] = 0;
	vCoordinates[2] = z;
	
	vLocalPoints.push_back(vCoordinates);
	
	
	// -- Grid -- //	
	vector<vector<int> > grid;
	vector<int> line;
	
	for (j = 0; j < mm - 1; ++j)
	{
		line.clear();
		
		for (i = 1; i < nn + 1; ++i)
		{
			node = i + j * (nn);
			line.push_back(node);
		}
		line.push_back(line[0]);
		grid.push_back(line);
	}

	// -- Elements -- //
	// Triangles
	vElement.resize(3);
	//
	//   f ----- e
	//   |     / |
	//	 |   /   |
	//   | /     |
	//   c ----- d
	//
	
	int c, d, e, f;
	
	// Bottem Elements
	for (j = 0; j < nn; ++j)
	{
		c = 0;
		e = grid[0][j + 1];
		f = grid[0][j + 0];
		
		vElement[0] = c;
		vElement[1] = e;
		vElement[2] = f;
		vLocalElements.push_back(vElement);
	}
	
	// Middle Elements
	for (i = 0; i < mm - 2; ++i)
		for (j = 0; j < nn; ++j)
		{
			c = grid[i + 0][j + 0];
			d = grid[i + 0][j + 1];
			f = grid[i + 1][j + 0];
			e = grid[i + 1][j + 1];

			vElement[0] = c;
			vElement[1] = d;
			vElement[2] = e;
			vLocalElements.push_back(vElement);
			
			vElement[0] = e;
			vElement[1] = f;
			vElement[2] = c;
			vLocalElements.push_back(vElement);
		}

	// Bottem Elements
	for (j = 0; j < nn; ++j)
	{
		c = grid[mm - 2][j + 0];
		d = grid[mm - 2][j + 1];
		e = nnodes - 1;
		
		vElement[0] = c;
		vElement[1] = d;
		vElement[2] = e;
		vLocalElements.push_back(vElement);
	}
	
	/*
	//Quadrangles
	vCoordinates.resize(3);
	
	for (j = 0; j < mm + 1; ++j)
	{
		for (i = 0; i < nn + 1; ++i)
		{
			u = (pii * i) / nn;
			v = (2 * pii * j) / mm;
			
			node = i + j * (nn + 1);
			
			x = rx * sgn(cos(v)) * pow(fabs(cos(v)), aa) * pow(fabs(sin(u)), bb);
			y = ry * sgn(sin(v)) * pow(fabs(sin(v)), aa) * pow(fabs(sin(u)), bb);
			z = rz * sgn(cos(u)) * pow(fabs(cos(u)), bb);
			
			vCoordinates[0] = x;
			vCoordinates[1] = y;
			vCoordinates[2] = z;
			
			vLocalPoints.push_back(vCoordinates);
		}
	}
	
	vElement.resize(4);
	
	for (j = 0; j < mm; ++j)
	{
		for (i = 0; i < nn; ++i)
		{
			elem = i + j * nn;
			
			vElement[0] = elem + j;
			vElement[1] = elem + j + 1;
			vElement[2] = elem + j + nn + 2;
			vElement[3] = elem + j + nn + 1;
			
			vLocalElements.push_back(vElement);
		}
	}
	*/
	
	nLocalPoints = vLocalPoints.size();
	nLocalElements = vLocalElements.size();
}

// ***
double SurfaceSuperEllipsoid::evalS(double& x, double& y, double& z)
{
	S = pow(pow(fabs(x / rx), 2 / aa) + pow(fabs(y / ry), 2 / aa), aa / bb);
	S+= pow(fabs(z / rz), 2 / bb) - 1.0;

	return S;
}

//***
Eigen::RowVector3d SurfaceSuperEllipsoid::evaldS(double& x, double& y, double& z)
{
	dS.x() = pow(pow(fabs(x / rx), 2 / aa) + pow(fabs(y / ry), 2 / aa), aa / bb - 1);
	dS.x()*= sgn(x) * pow(fabs(x / rx), 2 / aa - 1) * 2 / (bb * rx);
	
	dS.y() = pow(pow(fabs(x / rx), 2 / aa) + pow(fabs(y / ry), 2 / aa), aa / bb - 1);
	dS.y()*= sgn(y) * pow(fabs(y / ry), 2 / aa - 1) * 2 / (bb * ry);
	
	dS.z() = sgn(z) * pow(fabs(z / rz), 2 / bb - 1) * 2 / (bb * rz);

	if(fabs(dS.x()) < 1E-6)
		dS.x() *= 0.;

	if(fabs(dS.y()) < 1E-6)
		dS.y() *= 0.;

	if(fabs(dS.z()) < 1E-6)
		dS.z() *= 0.;

	//cout << "dS.z(): " << dS.z() << endl;
	return dS;
}

//***
Eigen::Matrix3d SurfaceSuperEllipsoid::evalddS(double& x, double& y, double& z)
{
	ddS(0,0) = 2.0 / (rx * rx);
	ddS(1,1) = 2.0 / (ry * ry);
	ddS(2,2) = 2.0 / (rz * rz);	
/*
	double da;
	double db;
	double dc;
 
	da = pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 1);
	da*= 4 * pow(x / rx, 2 / aa) / (aa * bb * x * x);
	db = pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 1);
	db*= 2 * pow(x / rx, 2 / aa) / (bb * x * x);
	dc = pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 2);
	dc*= 4 * (aa / bb - 1) * pow(x / rx, 4 / aa) / (aa * bb * x * x);

	ddS[0][0] = da - db + dc;
	
	da= pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 1);
	da*= 4 * pow(y / ry, 2 / aa) / (aa * bb * y * y);
	db= pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 1);
	db*= 2 * pow(y / ry, 2 / aa) / (bb * y * y);
	dc= pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 2);
	dc*= 4 * (aa / bb - 1) * pow(y / ry, 4 / aa) / (aa * bb * y * y);
	
	ddS[1][1] = da - db + dc;
	
	da = 4 * pow(z / rz, 2 / bb) / (bb * bb * z * z);
	db = 2 * pow(z / rz, 2 / bb) / (bb * z * z);
	
	ddS[2][2] = da - db;
	
	ddS[0][1] = pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 2);
	ddS[0][1]*= 4 * (aa / bb - 1) / (aa * bb * x * y);
	ddS[0][1]*= pow(x / rx, 2 / aa);
	ddS[0][1]*= pow(y / ry, 2 / aa);

	ddS[1][0] = pow(pow(x / rx, 2 / aa) + pow(y / ry, 2 / aa), aa / bb - 2);
	ddS[1][0]*= 4 * (aa / bb - 1) / (aa * bb * x * y);
	ddS[1][0]*= pow(x / rx, 2 / aa);
	ddS[1][0]*= pow(y / ry, 2 / aa);
*/	
	return ddS;
}
